#include "PowerFlowCalculation.h"
#include "gsl/gsl_complex.h"
#include "gsl/gsl_complex_math.h"


gsl_matrix* gsl_matrix_complex_real(gsl_matrix_complex* m){
    gsl_matrix* res=gsl_matrix_alloc(m->size1,m->size2);
    for(int i=0;i<m->size1;i++){
        for(int j=0;j<m->size2;j++){
            gsl_matrix_set(res,i,j,GSL_REAL(gsl_matrix_complex_get(m,i,j)));
        }
    }
    return res;
}
gsl_matrix* gsl_matrix_complex_imag(gsl_matrix_complex* m){
    gsl_matrix* res=gsl_matrix_alloc(m->size1,m->size2);
    for(int i=0;i<m->size1;i++){
        for(int j=0;j<m->size2;j++){
            gsl_matrix_set(res,i,j,GSL_IMAG(gsl_matrix_complex_get(m,i,j)));
        }
    }
    return res;
}
FormGB FormGB_former(gsl_matrix* TransFormer_Branch,gsl_matrix *Normal_Branch,gsl_matrix* PQ_Node,gsl_matrix * PV_Node,int Node_Num,Node m_Node[])
{
    FormGB fgb;
    fgb.G=gsl_matrix_alloc(Node_Num,Node_Num);
    fgb.B=gsl_matrix_alloc(Node_Num,Node_Num);
    gsl_matrix_complex *GB=gsl_matrix_complex_alloc(Node_Num,Node_Num);
    gsl_matrix_complex *GB1=gsl_matrix_complex_alloc(Node_Num,Node_Num);
    for(int m=0;m<TransFormer_Branch->size1;m++){
        int i=gsl_matrix_get(TransFormer_Branch,m,0)-1;
        int j=gsl_matrix_get(TransFormer_Branch,m,1)-1;
        gsl_complex RX=gsl_complex_rect(0,gsl_matrix_get(TransFormer_Branch,m,2));
        gsl_complex k=gsl_complex_rect(gsl_matrix_get(TransFormer_Branch,m,3),0);

        gsl_complex temp=gsl_complex_rect(-1,0);
        temp=gsl_complex_div(temp,RX);
        temp=gsl_complex_div(temp,k);
        gsl_matrix_complex_set(GB1,i,j,temp);
        temp=gsl_complex_add(gsl_matrix_complex_get(GB,i,j),temp);
        gsl_matrix_complex_set(GB,i,j,temp);
        gsl_matrix_complex_set(GB,j,i,gsl_matrix_complex_get(GB,i,j));

        gsl_matrix_complex_set(GB,i,i,gsl_complex_sub(gsl_matrix_complex_get(GB,i,i),gsl_matrix_complex_get(GB1,i,j)));
        temp=gsl_complex_sub(k,gsl_complex_rect(1,0));
        temp=gsl_complex_div(temp,k);
        temp=gsl_complex_div(temp,RX);
        gsl_matrix_complex_set(GB,i,i,gsl_complex_add(gsl_matrix_complex_get(GB,i,i),temp));
        //temp=gsl_complex_sub(temp,gsl_complex_div(gsl_complex_rect(1,0),));
        gsl_matrix_complex_set(GB,j,j,gsl_complex_sub(gsl_matrix_complex_get(GB,j,j),gsl_matrix_complex_get(GB1,i,j)));
        temp=gsl_complex_sub(gsl_complex_rect(1,0),k);
        temp=gsl_complex_div(temp,k);
        temp=gsl_complex_div(temp,k);
        temp=gsl_complex_div(temp,RX);
        gsl_matrix_complex_set(GB,j,j,gsl_complex_add(gsl_matrix_complex_get(GB,j,j),temp));
    }
    

    for(int m=0;m<Normal_Branch->size1;m++){
        int i=gsl_matrix_get(Normal_Branch,m,0)-1;
        int j=gsl_matrix_get(Normal_Branch,m,1)-1;
        gsl_complex RX=gsl_complex_rect(gsl_matrix_get(Normal_Branch,m,2),gsl_matrix_get(Normal_Branch,m,3));
        gsl_complex B_2=gsl_complex_rect(0,gsl_matrix_get(Normal_Branch,m,4));
        gsl_complex Yij=gsl_complex_div(gsl_complex_rect(-1,0),RX);
        gsl_matrix_complex_set(GB,i,j,gsl_complex_add(gsl_matrix_complex_get(GB,i,j),Yij));
        gsl_matrix_complex_set(GB,j,i,gsl_matrix_complex_get(GB,i,j));
        gsl_complex temp=gsl_complex_add(Yij,B_2);
        gsl_matrix_complex_set(GB,i,i,gsl_complex_sub(gsl_matrix_complex_get(GB,i,i),temp));
        gsl_matrix_complex_set(GB,j,j,gsl_complex_sub(gsl_matrix_complex_get(GB,j,j),temp));
    }

    for(int m=0;m<PQ_Node->size1;m++){
        int i=gsl_matrix_get(PQ_Node,m,0);
        gsl_complex temp=gsl_complex_rect(0,gsl_matrix_get(PQ_Node,m,5));
        gsl_matrix_complex_set(GB,i,i,gsl_complex_add(gsl_matrix_complex_get(GB,i,i),temp));
    }
    for(int m=0;m<PV_Node->size1;m++){
        int i=gsl_matrix_get(PV_Node,m,0);
        gsl_complex temp=gsl_complex_rect(0,gsl_matrix_get(PV_Node,m,7));
        gsl_matrix_complex_set(GB,i,i,gsl_complex_add(gsl_matrix_complex_get(GB,i,i),temp));
    }
    gsl_matrix_complex *GB2=gsl_matrix_complex_alloc(Node_Num,Node_Num);
    gsl_matrix_complex_memcpy(GB2,GB);
    for(int i=0;i<Node_Num;i++){
        int v=m_Node[i].NodeID-1;
        for(int j=0;j<Node_Num;j++){
            int w=m_Node[j].NodeID-1;
            
            gsl_matrix_complex_set(GB,i,j,gsl_matrix_complex_get(GB2,v,w));
        }
        //printf("v=%d\n",v);
    }



    gsl_matrix *mr=gsl_matrix_complex_real(GB);
    gsl_matrix *mi=gsl_matrix_complex_imag(GB);
    // PRINT_MATRIX(mr)

    // printf("\n");
    // PRINT_MATRIX(mi)
    fgb.G=mr;
    fgb.B=mi;
    return fgb;
}

